﻿$import( "SmitearWebGL.Core.Math.Matrix4x4" );

SmitearWebGL.Core.Math.Quaternion = Class.extend(
{
	/* * * * * * * * constructor * * * * * * * */

	init : function( x, y, z, w )
	{
		this.x = x == undefined ? 0 : x;
		this.y = y == undefined ? 0 : y;
		this.z = z == undefined ? 0 : z;
		this.w = w == undefined ? 1 : w;
		
		this._registClass( "SmitearWebGL.Core.Math.Quaternion" );
	}
	,
	
	/******** public method ********/
	
	reset : function( x, y, z, w )
	{
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}
	,
	clone : function()
	{
		return $Q( this.x, this.y, this.z, this.w );
	}
	,
	copy : function( q )
	{
		this.x = q.x;
		this.y = q.y;
		this.z = q.z;
		this.w = q.w;
	}
	,
	identity : function()
	{
		this.x = 0.0;
		this.y = 0.0;
		this.z = 0.0;
		this.w = 1.0;
	}
	,
	scale : function( s )
	{
		this.x *= s;
		this.y *= s;
		this.z *= s;
		this.w *= s;
	}
	,
	conjugate : function()
	{
		this.x = - this.x;
		this.y = - this.y;
		this.z = - this.z;
	}
	,
	moduloSquared : function()
	{
		return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
	}
	,
	magnitude : function()
	{
		var n = this.moduloSquared();
		if ( n > 0.0 )
		return Math.sqrt( n );
		else
		return 0.0;
	}
	,
	normalize : function()
	{
		var l = this.magnitude();
		if ( l > 0.0 )
		this.scale( 1.0 / l );
		else
		this.identity();
	}
	,
	invert : function()
	{
		var n = this.moduloSquared();
		if ( n > 0.0 ) this.scale( 1.0 / n );
		this.conjugate();
	}
	,
	sub : function( a, b )
	{
		return $Q( a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w );
	}
	,
	add : function( a, b )
	{
		return $Q( a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w );
	}
	,
	dot : function( a, b )
	{
		return ( a.x * b.x ) + ( a.y * b.y ) + ( a.z * b.z ) + ( a.w * b.w );
	}
	,
	multiply : function( b )
	{
		var aw = this.w;
		var ax = this.x;
		var ay = this.y;
		var az = this.z;

		this.x = aw * b.x + ax * b.w + ay * b.z - az * b.y;
		this.y = aw * b.y - ax * b.z + ay * b.w + az * b.x;
		this.z = aw * b.z + ax * b.y - ay * b.x + az * b.w;
		this.w = aw * b.w - ax * b.x - ay * b.y - az * b.z;
	}
	,
	getMatrix : function()
	{
		var wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		x2 = this.x + this.x;
		y2 = this.y + this.y;
		z2 = this.z + this.z;
		xx = this.x * x2;
		xy = this.x * y2;
		xz = this.x * z2;
		yy = this.y * y2;
		yz = this.y * z2;
		zz = this.z * z2;
		wx = this.w * x2;
		wy = this.w * y2;
		wz = this.w * z2;

		return $M4x4( [[1.0 - ( yy + zz ), xy + wz, xz - wy, 0.0],
		[xy - wz, 1.0 - ( xx + zz ), yz + wx, 0.0],
		[xz + wy, yz - wx, 1.0 - ( xx + yy ), 0.0],
		[0.0, 0.0, 0.0, 1.0]] );
	}
	,
	getEuler : function( euler )
	{
		var test = this.x * this.y + this.z * this.w;
		
		if ( test > 0.499 )
		{
			euler.y = 2 * Math.atan2( this.x, this.w );
			euler.z = Math.PI / 2;
			euler.x = 0;
			return;
		}
		
		if ( test < - 0.499 )
		{
			euler.y = - 2 * Math.atan2( this.x, this.w );
			euler.z = - Math.PI / 2;
			euler.x = 0;
			return;
		}
		
		var sqx = this.x * this.x;
		var sqy = this.y * this.y;
		var sqz = this.z * this.z;
		
		euler.y = Math.atan2( 2 * this.y * this.w - 2 * this.x * this.z , 1 - 2 * sqy - 2 * sqz );
		euler.z = Math.asin( 2 * test );
		euler.x = Math.atan2( 2 * this.x * this.w - 2 * this.y * this.z , 1 - 2 * sqx - 2 * sqz );
		
		return euler;
	}
	,
	setFromYawPitchRoll : function( yaw, pitch, roll )
	{
		var thetaOver2 = yaw * 0.5;
		var c1 = Math.cos( thetaOver2 );
		var s1 = Math.sin( thetaOver2 );

		thetaOver2 = roll * 0.5;
		var c2 = Math.cos( thetaOver2 );
		var s2 = Math.sin( thetaOver2 );

		thetaOver2 = pitch * 0.5;
		var c3 = Math.cos( thetaOver2 );
		var s3 = Math.sin( thetaOver2 );

		var c1c2 = c1 * c2;
		var s1s2 = s1 * s2;

		this.w = c1c2 * c3 - s1s2 * s3;
		this.x = c1c2 * s3 + s1s2 * c3;
		this.y = s1 * c2 * c3 + c1 * s2 * s3;
		this.z = c1 * s2 * c3 - s1 * c2 * s3;
	}
	,
	setRotateX : function( pitch )
	{
		var thetaOver2 = pitch * 0.5;
		var sin_a = Math.sin( thetaOver2 );
		this.w = Math.cos( thetaOver2 );
		this.x = sin_a;
		this.y = 0.0;
		this.z = 0.0;
	}
	,
	setRotateY : function( yaw )
	{
		var thetaOver2 = yaw * 0.5;
		var sin_a = Math.sin( thetaOver2 );
		this.w = Math.cos( thetaOver2 );
		this.x = 0.0;
		this.y = sin_a;
		this.z = 0.0;
	}
	,
	setRotateZ : function( roll )
	{
		var thetaOver2 = roll * 0.5;
		var sin_a = Math.sin( thetaOver2 );
		this.w = Math.cos( thetaOver2 );
		this.x = 0.0;
		this.y = 0.0;
		this.z = sin_a;
	}
	,
	setRotateAxisAngle : function( v, theta )
	{
		var thetaOver2 = theta * 0.5;
		var sin_a = Math.sin( thetaOver2 );
		this.w = Math.cos( thetaOver2 );
		this.x = v.x * sin_a;
		this.y = v.y * sin_a;
		this.z = v.z * sin_a;
	}
	,
	rotateAxis : function( v )
	{
		var q = $Q( v.x * this.w + v.z * this.y - v.y * this.z,
		v.y * this.w + v.x * this.z - v.z * this.x,
		v.z * this.w + v.y * this.x - v.x * this.y,
		v.x * this.x + v.y * this.y + v.z * this.z );

		return $V3( w * q.x + this.x * q.w + this.y * q.z - this.z * q.y,
		w * q.y + this.y * q.w + this.z * q.x - this.x * q.z,
		w * q.z + this.z * q.w + this.x * q.y - this.y * q.x );
	}
	,
	setFromEuler : function( ax, ay, az )
	{
		var fSinPitch        = Math.sin( ax * 0.5 );
		var fCosPitch        = Math.cos( ax * 0.5 );
		var fSinYaw          = Math.sin( ay * 0.5 );
		var fCosYaw          = Math.cos( ay * 0.5 );
		var fSinRoll         = Math.sin( az * 0.5 );
		var fCosRoll         = Math.cos( az * 0.5 );
		var fCosPitchCosYaw  = fCosPitch * fCosYaw;
		var fSinPitchSinYaw  = fSinPitch * fSinYaw;

		this.x = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw;
		this.y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
		this.z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
		this.w = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw;
	}
	,
	setFromAxis : function( from, to )
	{
		var c = SmitearWebGL.Core.Math.Vector3.cross( from, to );
		this.x = c.x;
		this.y = c.y;
		this.z = c.z;
		this.w = from.x * to.x + from.y * to.y + from.z * to.z;
		w += 1.0;
		if ( w <= 0.000001 )
		{
			if ( ( from.z * from.z ) > ( from.x * from.x ) )
			{
				this.x = 0.0;
				this.y = from.z;
				this.z = - from.y;
				this.w = w;
			}
			else
			{
				this.x = from.y;
				this.y = - from.x;
				this.z = 0.0;
				this.w = w;
			}
		}
		this.normalize();
	}
	,
	setFromRotationMatrix : function( pm )
	{
		var maxi;
		var maxdiag, s, trace;

		trace = pm.n11 + pm.n22 + pm.n33 + 1.0;

		if ( trace > 1.0 )
		{
			this.x = ( pm.n23 - pm.n32 ) / ( 2.0 * Math.sqrt( trace ) );
			this.y = ( pm.n31 - pm.n13 ) / ( 2.0 * Math.sqrt( trace ) );
			this.z = ( pm.n12 - pm.n21 ) / ( 2.0 * Math.sqrt( trace ) );
			this.w = Math.sqrt( trace ) / 2.0;
			return;
		}

		maxi = 0;
		maxdiag = pm.n11;

		if ( pm.n22 > maxdiag )
		{
			maxi = 1;
			maxdiag = pm.n22;
		}

		if ( pm.n33 > maxdiag )
		{
			maxi = 2;
			maxdiag = pm.n33;
		}

		switch( maxi )
		{
			case 0 :
			s = 2.0 * Math.sqrt( 1.0 + pm.n11 - pm.n22 - pm.n33 );
			this.x = 0.25 * s;
			this.y = ( pm.n12 + pm.n21 ) / s;
			this.z = ( pm.n13 + pm.n31 ) / s;
			this.w = ( pm.n23 - pm.n32 ) / s;
			break;
			case 1 :
			s = 2.0 * Math.sqrt( 1.0 + pm.n22 - pm.n11 - pm.n33 );
			this.x = ( pm.n12 + pm.n21 ) / s;
			this.y = 0.25 * s;
			this.z = ( pm.n23 + pm.n32 ) / s;
			this.w = ( pm.n31 - pm.n13 ) / s;
			break;
			case 2 :
			s = 2.0 * Math.sqrt( 1.0 + pm.n33 - pm.n11 - pm.n22 );
			this.x = ( pm.n13 + pm.n31 ) / s;
			this.y = ( pm.n23 + pm.n32 ) / s;
			this.z = 0.25 * s;
			this.w = ( pm.n12 - pm.n21 ) / s;
			break;
		}
	}
	,
	slerp : function( q0, q1, l )
	{
		var fScale1;
		var fScale2;
		var A = q0.clone();
		var B = q1.clone();

		// compute dot product, aka COSINE( theta ) :
		var fCosTheta = A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w;

		if ( fCosTheta < 0.0 )
		{
			// flip start quaternion
			A.x = - A.x;
			A.y = - A.y;
			A.z = - A.z;
			A.w = - A.w;
			fCosTheta = - fCosTheta;
		}

		if ( ( fCosTheta + 1.0 ) > 0.05 )
		{
			// If the quaternions are close, use linear interploation
			if ( ( 1.0 - fCosTheta ) < 0.05 )
			{
				fScale1 = 1.0 - l;
				fScale2 = l;
			}
			else
			{
				// Otherwise, do spherical interpolation
				var fTheta    = Math.acos( fCosTheta );
				var fSinTheta = Math.sin( fTheta );
				fScale1 = Math.sin( fTheta * ( 1.0 - l ) ) / fSinTheta;
				fScale2 = Math.sin( fTheta * l ) / fSinTheta;
			}
		}
		else
		{
			B.x = - A.y;
			B.y =  A.x;
			B.z = - A.w;
			B.w =  A.z;
			fScale1 = Math.sin( Math.PI * ( 0.5 - l ) );
			fScale2 = Math.sin( Math.PI * l );
		}

		this.x = fScale1 * A.x + fScale2 * B.x;
		this.y = fScale1 * A.y + fScale2 * B.y;
		this.z = fScale1 * A.z + fScale2 * B.z;
		this.w = fScale1 * A.w + fScale2 * B.w;
	}
}
);

SmitearWebGL.Core.Math.Quaternion.create = function( x, y, z, w )
{
	return new SmitearWebGL.Core.Math.Quaternion( x, y, z, w );
}

SmitearWebGL.Core.Math.Quaternion.multiply = function( a, b )
{
	var c = $Q();
	c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
	c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
	c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
	c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
	return c;
}

var $Q = SmitearWebGL.Core.Math.Quaternion.create;